﻿/*
 * kalman.c
 *
 * Created: 2013-08-22 오전 8:25:36
 *  Author: akkdong
 */ 

#include <math.h>
#include "kalman.h"

void kalman_init(KalmanFilter * handle, double var_accel)
{
	kalman_setAccelerationVariance(handle, var_accel);
	kalman_reset(handle, 0.0, 0.0);
}

void kalman_reset(KalmanFilter * handle, double abs_value, double vel_value)
{
	handle->x_abs = abs_value;
	handle->x_vel = vel_value;
	handle->p_abs_abs = 1.0e10;
	handle->p_abs_vel = 0.0;
	handle->p_vel_vel = handle->var_accel;
}

void kalman_setAccelerationVariance(KalmanFilter * handle, double var_accel)
{
	handle->var_accel = var_accel;
}

// Updates state given a sensor measurement of the absolute value of x,
// the variance of that measurement, and the interval since the last
// measurement in seconds. This interval must be greater than 0; for the
// first measurement after a reset(), it's safe to use 1.0.
void kalman_update(KalmanFilter * handle, double z_abs, double var_z_abs, double dt)
{
	// Note: math is not optimized by hand. Let the compiler sort it out.
	// Predict step.
	// Update state estimate.
	handle->x_abs += handle->x_vel * dt;
	// Update state covariance. The last term mixes in acceleration noise.
	handle->p_abs_abs += 2.0 * dt * handle->p_abs_vel + dt * dt * handle->p_vel_vel + handle->var_accel * dt * dt * dt * dt / 4.0;
	handle->p_abs_vel += dt * handle->p_vel_vel + handle->var_accel * dt * dt * dt / 2.0;
	handle->p_vel_vel += handle->var_accel * dt * dt;

	// Update step.
	handle->y = z_abs - handle->x_abs;						// Innovation.
	handle->s_inv = 1. / (handle->p_abs_abs + var_z_abs);	// Innovation precision.
	handle->k_abs = handle->p_abs_abs * handle->s_inv;		// Kalman gain
	handle->k_vel = handle->p_abs_vel * handle->s_inv;
	
	// Update state estimate.
	handle->x_abs += handle->k_abs * handle->y;
	handle->x_vel += handle->k_vel * handle->y;
	
	// Update state covariance.
	handle->p_vel_vel -= handle->p_abs_vel * handle->k_vel;
	handle->p_abs_vel -= handle->p_abs_vel * handle->k_abs;
	handle->p_abs_abs -= handle->p_abs_abs * handle->k_abs;
}

double kalman_getXAbs(KalmanFilter * handle)
{
	return handle->x_abs;
}

double kalman_getXVel(KalmanFilter * handle)
{
	return handle->x_vel;
}

double kalman_getCovAbsAbs(KalmanFilter * handle)
{
	return handle->p_abs_abs;
}

double kalman_getCovAbsVel(KalmanFilter * handle)
{
	return handle->p_abs_vel;
}

double kalman_getCovVelVel(KalmanFilter * handle)
{
	return handle->p_vel_vel;
}
